package com.gitee.wsl.mathematics.matrix.mat4.ext

import com.gitee.wsl.mathematics.coordinate.d3.Point3
import com.gitee.wsl.mathematics.matrix.ext.setIdentity
import com.gitee.wsl.mathematics.matrix.mat4.MutableMatrix4
import com.gitee.wsl.mathematics.number.compareTo
import com.gitee.wsl.mathematics.vector.ext.cross
import com.gitee.wsl.mathematics.vector.ext.dot
import com.gitee.wsl.mathematics.vector.ext.minus

/**
 * Sets the matrix to a look at matrix with a direction and an up vector. Multiply with a
 * translation matrix to get a camera model view matrix.
 *
 * @param direction the direction vector
 * @param up the up vector
 * @return this matrix
 */
fun<T:Number,R:Point3<T,R>> MutableMatrix4<T, *, *>.setLookAt(direction: Point3<T,R>, up: Point3<T,R>){
    val l_vez = direction.normalized
    val l_vex = l_vez.cross(up).normalized
    val l_vey = l_vex.cross(l_vez).normalized
    /*l_vez.apply {
        set(direction)
        norm()
    }
    l_vex.apply {
        set(direction)
        norm()
        set(cross(up))
        norm()
    }
    l_vey.apply {
        set(l_vex)
        set(cross(l_vez))
        norm()
    }*/

    setIdentity()

    m00 = l_vex.x
    m01 = l_vex.y
    m02 = l_vex.z

    m10 = l_vey.x
    m11 = l_vey.y
    m12 = l_vey.z

    m20 = -l_vez.x
    m21 = -l_vez.y
    m22 = -l_vez.z

}

/**
 * Sets this matrix to a look at matrix with the given position, target and up vector.
 *
 * @param position the position
 * @param lookAt the target
 * @param up the up vector
 * @return this matrix
 */
fun<T:Number,R:Point3<T,R>> MutableMatrix4<T, *, *>.setLookAt(position: Point3<T,R>, lookAt: Point3<T,R>, up: Point3<T,R>) {
    // See the OpenGL GLUT documentation for gluLookAt for a description
    // of the algorithm. We implement it in a straightforward way:
    var fx = lookAt.x - position.x
    var fy = lookAt.y - position.y
    var fz = lookAt.z - position.z

    // Normalize f
    val rlf = 1f / sqrt(fx * fx + fy * fy + fz * fz)
    fx *= rlf
    fy *= rlf
    fz *= rlf

    // compute s = f x up (x means "cross product")
    var sx = fy * up.z - fz * up.y
    var sy = fz * up.x - fx * up.z
    var sz = fx * up.y - fy * up.x

    // and normalize s
    val rls = 1f / sqrt(sx * sx + sy * sy + sz * sz)
    sx *= rls
    sy *= rls
    sz *= rls

    // compute u = s x f
    val ux = sy * fz - sz * fy
    val uy = sz * fx - sx * fz
    val uz = sx * fy - sy * fx

    m00 = sx
    m10 = ux
    m20 = -fx
    m30 = zero

    m01 = sy
    m11 = uy
    m21 = -fy
    m31 = zero

    m02 = sz
    m12 = uz
    m22 = -fz
    m32 = zero

    m03 = zero
    m13 = zero
    m23 = zero
    m33 = one

    setTranslate(-position.x, -position.y, -position.z)
}

fun<T:Number,R:Point3<T,R>> MutableMatrix4<T, *, *>.setToWorld(position: Point3<T,R>, forward: Point3<T,R>, up: Point3<T,R>) {
    val tmpForward = forward.normalized

    val right = tmpForward.cross(up).normalized
    val tmpUp = right.cross(tmpForward).normalized
    val forward = tmpForward * -1f
    set(right, tmpUp, forward, position)
}

/**
 * Inplace operation: Applies a look-at transform to this matrix.
 */
fun<T:Number,R:Point3<T,R>> MutableMatrix4<T, *, *>.lookAt(eyePosition: Point3<T,R>, lookAt: Point3<T,R>, up: Point3<T,R>) {
    var z = lookAt - eyePosition
    val fLen = z.length

    if(fLen.isApproxZero()) {
        // eye position and look at are equal
        z = z.create(z.x,z.y,one)
    } else {
        z *= (1f / fLen)
    }

    var x = z.cross(up)
    val sLen = x.length
    if (sLen.isApproxZero()) {
        val Y_Axis = z.create(zero, one, zero)
        // forward vector is parallel to up
        if (abs(z.dot(Y_Axis)) > 0.99f) {
            val NEG_Z_AXIS=z.create(zero, zero, -one)
            x = z.cross(NEG_Z_AXIS)
            x = x.normalized
        } else {
            x = z.cross(Y_Axis)
            x = x.normalized
        }
    } else {
        x *= 1f / sLen
    }

    val y = x.cross(z)

    return mul(
        x.x, x.y, x.z, -x.dot(eyePosition),
        y.x, y.y, y.z, -y.dot(eyePosition),
        -z.x, -z.y, -z.z, z.dot(eyePosition),
        zero, zero, zero, one
    )
}
